﻿using Kimd.Machine;
using Kimd.Motion;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace WorkStation.Logic
{
    public class DDMotor
    {
        private Pdo _controlAddress;
        private Pdo _dataAddress1;
        private Pdo _dataAddress2;
        private List<Cylinder> _loadLiftCylinders;
        private List<Cylinder> _unloadLiftCylinders;
        public DDMotor(Pdo control, Pdo data1, Pdo data2, List<Cylinder> cylinders1, List<Cylinder> cylinders2)
        {
            this._controlAddress = control;
            this._dataAddress1 = data1;
            this._dataAddress2 = data2;
            this._loadLiftCylinders = cylinders1;
            this._unloadLiftCylinders = cylinders2;
        }
        public bool RotateOnce(out string errInfo)
        {
            errInfo = "";
            if (_loadLiftCylinders != null && _loadLiftCylinders.Count > 0)
            {
                foreach (Cylinder cylinder in _loadLiftCylinders)
                {
                    if (cylinder != null)
                    {
                        if (!cylinder.WaitOff(5000))
                        {
                            errInfo = string.Format("等待{0}感应器ON超时!", cylinder.Description);
                            return false;
                        }
                    }
                }
            }
            if (_unloadLiftCylinders != null && _unloadLiftCylinders.Count > 0)
            {
                foreach (Cylinder cylinder in _unloadLiftCylinders)
                {
                    if (cylinder != null)
                    {
                        if (!cylinder.WaitOff(5000))
                        {
                            errInfo = string.Format("等待{0}感应器ON超时!", cylinder.Description);
                            return false;
                        }
                    }
                }
            }
            if(!MachineResource.Instance.MachineSensor["ST3上下气缸上位"].WaitOn())
            {
                errInfo = "ST3上下气缸不在上位，不能旋转";
                return false;
            }
            if (!MachineResource.Instance.MachineSensor["ST5上下气缸上位"].WaitOn())
            {
                errInfo = "ST5上下气缸不在上位，不能旋转";
                return false;
            }
            if (MachineResource.Instance.MachineAxis["上料X轴"].IsNearPosition("转盘放料位置", 10) &&
                MachineResource.Instance.MachineAxis["上料Y轴"].IsNearPosition("转盘放料位置", 10) &&
                MachineResource.Instance.MachineAxis["上料Z轴"].IsNearPosition("转盘放料位置", 2))
            {
                if(MachineResource.Instance.MachineSensor["上料四轴上下气缸1下位"].WaitOn() ||
                   MachineResource.Instance.MachineSensor["上料四轴上下气缸2下位"].WaitOn())
                {
                    errInfo = "上料四轴在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机左"].IsNearPosition("转盘放料位置左", 4) &&
               MachineResource.Instance.MachineAxis["测试X轴左"].IsNearPosition("转盘放料位置左", 4) &&
               MachineResource.Instance.MachineAxis["测试Y轴左"].IsNearPosition("转盘放料位置左", 4) &&
               MachineResource.Instance.MachineAxis["测试Z轴左"].IsNearPosition("转盘放料位置左", 2))
            {
                if (MachineResource.Instance.MachineSensor["左测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "左测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机左"].IsNearPosition("转盘取料位置左", 4) &&
                MachineResource.Instance.MachineAxis["测试X轴左"].IsNearPosition("转盘取料位置左", 4) &&
                MachineResource.Instance.MachineAxis["测试Y轴左"].IsNearPosition("转盘取料位置左", 4) &&
                MachineResource.Instance.MachineAxis["测试Z轴左"].IsNearPosition("转盘取料位置左", 2))
            {
                if (MachineResource.Instance.MachineSensor["左测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "左测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机右"].IsNearPosition("转盘放料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试X轴右"].IsNearPosition("转盘放料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Y轴右"].IsNearPosition("转盘放料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Z轴右"].IsNearPosition("转盘放料位置右", 2))
            {
                if (MachineResource.Instance.MachineSensor["右测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "右测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机右"].IsNearPosition("转盘取料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试X轴右"].IsNearPosition("转盘取料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Y轴右"].IsNearPosition("转盘取料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Z轴右"].IsNearPosition("转盘取料位置右", 2))
            {
                if (MachineResource.Instance.MachineSensor["右测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "右测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            _controlAddress.SetData(768);
            Thread.Sleep(50);
            _controlAddress.SetData(512);
            DateTime startTime = DateTime.Now;
            TimeSpan ts = DateTime.Now - startTime;
            while(!IsInPosition() && ts.TotalMilliseconds < 5000)
            {
                ts = DateTime.Now - startTime;
                Thread.Sleep(10);
            }
            if(ts.TotalMilliseconds >= 5000)
                errInfo = "DD到位超时!";
            return true;
        }
        public void ServoOn()
        {
            _controlAddress.SetData(512);
        }
        public void ServoOff()
        {
            _controlAddress.SetData(0);
        }
        public void AlarmReset()
        {
            _controlAddress.SetData(64);
            Thread.Sleep(50);
            _controlAddress.SetData(0);
        }
        public bool GoHome(out string errInfo)
        {
            errInfo = "";
            if (_loadLiftCylinders != null && _loadLiftCylinders.Count > 0)
            {
                foreach (Cylinder cylinder in _loadLiftCylinders)
                {
                    if (cylinder != null)
                    {
                        if (!cylinder.WaitOff(5000))
                        {
                            errInfo = string.Format("等待{0}感应器ON超时!", cylinder.Description);
                            return false;
                        }
                    }
                }
            }
            if (_unloadLiftCylinders != null && _unloadLiftCylinders.Count > 0)
            {
                foreach (Cylinder cylinder in _unloadLiftCylinders)
                {
                    if (cylinder != null)
                    {
                        if (!cylinder.WaitOff(5000))
                        {
                            errInfo = string.Format("等待{0}感应器ON超时!", cylinder.Description);
                            return false;
                        }
                    }
                }
            }
            if (!MachineResource.Instance.MachineSensor["ST3上下气缸上位"].WaitOn())
            {
                errInfo = "ST3上下气缸不在上位，不能旋转";
                return false;
            }
            if (!MachineResource.Instance.MachineSensor["ST5上下气缸上位"].WaitOn())
            {
                errInfo = "ST5上下气缸不在上位，不能旋转";
                return false;
            }
            if (MachineResource.Instance.MachineAxis["上料X轴"].IsNearPosition("转盘放料位置", 10) &&
                MachineResource.Instance.MachineAxis["上料Y轴"].IsNearPosition("转盘放料位置", 10) &&
                MachineResource.Instance.MachineAxis["上料Z轴"].IsNearPosition("转盘放料位置", 2))
            {
                if (MachineResource.Instance.MachineSensor["上料四轴上下气缸1下位"].WaitOn() ||
                   MachineResource.Instance.MachineSensor["上料四轴上下气缸2下位"].WaitOn())
                {
                    errInfo = "上料四轴在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机左"].IsNearPosition("转盘放料位置左", 4) &&
               MachineResource.Instance.MachineAxis["测试X轴左"].IsNearPosition("转盘放料位置左", 4) &&
               MachineResource.Instance.MachineAxis["测试Y轴左"].IsNearPosition("转盘放料位置左", 4) &&
               MachineResource.Instance.MachineAxis["测试Z轴左"].IsNearPosition("转盘放料位置左", 2))
            {
                if (MachineResource.Instance.MachineSensor["左测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "左测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机左"].IsNearPosition("转盘取料位置左", 4) &&
                MachineResource.Instance.MachineAxis["测试X轴左"].IsNearPosition("转盘取料位置左", 4) &&
                MachineResource.Instance.MachineAxis["测试Y轴左"].IsNearPosition("转盘取料位置左", 4) &&
                MachineResource.Instance.MachineAxis["测试Z轴左"].IsNearPosition("转盘取料位置左", 2))
            {
                if (MachineResource.Instance.MachineSensor["左测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["左测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "左测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机右"].IsNearPosition("转盘放料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试X轴右"].IsNearPosition("转盘放料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Y轴右"].IsNearPosition("转盘放料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Z轴右"].IsNearPosition("转盘放料位置右", 2))
            {
                if (MachineResource.Instance.MachineSensor["右测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "右测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            if (MachineResource.Instance.MachineAxis["直线电机右"].IsNearPosition("转盘取料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试X轴右"].IsNearPosition("转盘取料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Y轴右"].IsNearPosition("转盘取料位置右", 4) &&
                MachineResource.Instance.MachineAxis["测试Z轴右"].IsNearPosition("转盘取料位置右", 2))
            {
                if (MachineResource.Instance.MachineSensor["右测试移载下料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载下料上下气缸2下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸1下位"].WaitOn() ||
                    MachineResource.Instance.MachineSensor["右测试移载上料上下气缸2下位"].WaitOn())
                {
                    errInfo = "右测试模组在危险位置，不能旋转";
                    return false;
                }
            }
            _controlAddress.SetData(640);
            Thread.Sleep(50);
            _controlAddress.SetData(512);
            DateTime startTime = DateTime.Now;
            TimeSpan ts = DateTime.Now - startTime;
            while (!IsInHome() && ts.TotalMilliseconds < 60000)
            {
                ts = DateTime.Now - startTime;
                Thread.Sleep(10);
            }
            if (ts.TotalMilliseconds >= 60000)
            {
                errInfo = "DD回零超时!";
                return false;
            }
            return true;
        }
        public double ReadPosition()
        {
            int position = 0;
            //读取前准备
            _dataAddress1.SetData(1);
            Thread.Sleep(50);
            _dataAddress2.SetData(1);
            Thread.Sleep(50);
            //读取值为角度*1000
            position = _dataAddress1.ReadData();
            double curPosition = position / 1000.00;
            _dataAddress1.SetData(0);
            _dataAddress2.SetData(0);
            return curPosition;
        }
        public bool IsInPosition(int timeOut = 5000)
        {
            Stopwatch sw = new Stopwatch();
            sw.Start();
            while (true)
            {
                int MDN = 0;
                //读取值为MDN
                MDN = _controlAddress.ReadData();
                Thread.Sleep(50);
                if ((MDN >> 10 & 1) == 1)
                {
                    return true;
                }
                if (sw.ElapsedMilliseconds > timeOut)
                {
                    return false;
                }
            }
        }
        public bool IsInHome()
        {
            int HMDN = _controlAddress.ReadData();
            Thread.Sleep(50);
            if ((HMDN >> 13 & 1) == 1 && (HMDN >> 10 & 1) == 1)
            {
                return true;
            }
            return false;
        }
    }
}
